package com.abe.config;

public class Interpolator {

	/**
	 * 这四个数是静态变量，需要给出初始值。可以通过后面两个值 改变其变化精度。先按我给出这个初始值来吧。
	 */
	private double x_last = 0;
	private double p_last = 5;
	private double kf_q = 0.001;
	private double kf_r = 0.1;

	/** 参数v是读取的高度值 */
	public double Kalman_filter(double v) {
		double x_mid;
		double x_now;
		double p_mid;
		double p_now;
		double kg;

		x_mid = x_last;
		p_mid = p_last + kf_q;
		kg = p_mid / (p_mid + kf_r);

		x_now = x_mid + kg * (v - x_mid);
		p_now = (1 - kg) * p_mid;

		/** update */
		p_last = p_now;
		x_last = x_now;

		return x_mid + kg * (v - x_mid);
	}

}
